/*******************************************************************
 *         Advanced 3D Game Programming using DirectX 8.0
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *   Title: matrix4.cpp
 *    Desc: Definition for a 4x4 homogenous matrix class
 *          
 * copyright (c) 2001 by Peter A Walsh and Adrian Perez
 * See license.txt for modification and distribution information
 ******************************************************************/

#include "matrix4.h"

const matrix4 matrix4::Identity = matrix4(
	1,0,0,0,
	0,1,0,0,
	0,0,1,0,
	0,0,0,1);

//returns matrix4*matrix4
matrix4 operator*(matrix4 const &a, matrix4 const &b)
{
    matrix4 out;      // temporary matrix4 for storing result
    for (int j = 0; j < 4; j++)         // transform by columns first
        for (int i = 0; i < 4; i++)     // then by rows
            out.m[i][j] = a.m[i][0] * b.m[0][j] + a.m[i][1] * b.m[1][j] +
                           a.m[i][2] * b.m[2][j] + a.m[i][3] * b.m[3][j];
	return out;
};

// Multiplies two matrices together
void MatMult( const matrix4 &a, const matrix4& b, matrix4* result )
{
    for (int j = 0; j < 4; j++)         // transform by columns first
        for (int i = 0; i < 4; i++)     // then by rows
            result->m[i][j] = a.m[i][0] * b.m[0][j] + a.m[i][1] * b.m[1][j] +
                              a.m[i][2] * b.m[2][j] + a.m[i][3] * b.m[3][j];
}


void matrix4::Rotate( float yaw, float pitch, float roll )
{
    int i;
    float sinangx,cosangx,sinangy,cosangy,sinangz,cosangz;
    float xold,yold,zold,xnew,ynew,znew;//temporary rotation results to prevent round off error
    sinangx=(float)sin(yaw);
    cosangx=(float)cos(yaw);
    sinangy=(float)sin(pitch);
    cosangy=(float)cos(pitch);
    sinangz=(float)sin(roll);
    cosangz=(float)cos(roll);
    for(i=0; i<3; i++)
    {
////////init points/////////////////////
        xold=m[i][0];
        yold=m[i][1];
        zold=m[i][2];
////////rotate point locally////////////        
        //xrot
        xnew=xold;
        ynew=(yold*cosangx)-(zold*sinangx);
        znew=(yold*sinangx)+(zold*cosangx);
        xold=xnew;
        yold=ynew;
        zold=znew;
        //yrot            
        xnew=((xold*cosangy)+(zold*sinangy));
        ynew=yold;
        znew=((-1*(xold*sinangy))+(zold*cosangy));
        xold=xnew;
        yold=ynew;
        zold=znew;
        //zrot
        xnew=((xold*cosangz)-(yold*sinangz));
        ynew=((yold*cosangz)+(xold*sinangz));
        znew=zold;
        //change object location
        m[i][0]=xnew;
        m[i][1]=ynew;
        m[i][2]=znew;
    }
}


void matrix4::MakeIdent()
{
	_11 = 1.f;	_22 = 1.f;	_33 = 1.f;	_44 = 1.f;
	_12 = 0.f;  _13 = 0.f;  _14 = 0.f;	_21 = 0.f;
	_23 = 0.f;	_24 = 0.f;	_31 = 0.f;	_32 = 0.f;	
	_34 = 0.f;	_41 = 0.f;	_42 = 0.f;	_43 = 0.f;	
}